﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;

namespace LocalizationTest
{
  class DiscreteAction1D : ActionDescription
  {
    public DiscreteAction1D(TypeOfAction typeOfAction)
    {
      this.typeOfAction = typeOfAction;
    }
    public enum TypeOfAction { moveLeft, moveRight, doNothing, none };
    public TypeOfAction typeOfAction;

    public override ActionDescription InvertAction()
    {
      DiscreteAction1D invertedAction = new DiscreteAction1D(TypeOfAction.doNothing);
      switch (typeOfAction)
      {
        case TypeOfAction.moveLeft: invertedAction.typeOfAction = TypeOfAction.moveRight; break;
        case TypeOfAction.moveRight: invertedAction.typeOfAction = TypeOfAction.moveLeft; break;
        default: invertedAction.typeOfAction = TypeOfAction.doNothing; break;
      }
      return invertedAction;
    }

    public override WorldState Execute(MotionModel motionModel, WorldState oldState)
    {
      if (Globals.selfLocatorProperties.MotionNoise == NoiseType.noNoise)
        return ExecutePerfect(motionModel, oldState);
      else
        return ExecuteWithError(motionModel, oldState);
    }

    public override WorldState ExecutePerfect(MotionModel motionModel, WorldState oldState)
    {
      double newPositionX = ((WorldState1D)oldState).Position;

      if (typeOfAction == TypeOfAction.moveRight) newPositionX += Globals.motionRasterWidth;
      if (typeOfAction == TypeOfAction.moveLeft) newPositionX -= Globals.motionRasterWidth;
      return new WorldState1D(newPositionX);
    }
    public WorldState ExecuteWithError(MotionModel motionModel, WorldState oldState)
    {
      Random random = new Random();
      double limit = random.NextDouble();
      double sum = 0.0;
      double newPosition = ((WorldState1D)oldState).Position;
      for (double deviation = -2 * Globals.motionRasterWidth; deviation <= 2 * Globals.motionRasterWidth; deviation += Globals.motionRasterWidth)
      {
        WorldState1D newState = new WorldState1D((oldState as WorldState1D).Position + deviation);
        double probability = motionModel.GetStateTransitionProbability(oldState, newState, this);
        sum += probability;
        if (limit < sum)
        {
          newPosition = newState.Position;
          break;
        }
      }
      if (newPosition < Globals.stateSpaceMinimumX)
        newPosition = Globals.stateSpaceMinimumX;
      if (newPosition > Globals.stateSpaceMaximumX)
        newPosition = Globals.stateSpaceMaximumX - (Globals.stateSpaceMaximumX - Globals.stateSpaceMinimumX) % Globals.motionRasterWidth;
      return new WorldState1D(newPosition);
    }
    public override string ToString()
    {
      throw new Exception("The method or operation is not implemented.");
    }

    public override ActionDescription Parse(string str)
    {
      throw new Exception("The method or operation is not implemented.");
    }
  }
}
